#include "gyre.h"

extern char pData[255];


#define get_lon 60
#define data_lon 11

Gyre_t Gyre;

void Gyre_Init(Gyre_t* Gyre)
{
	Get_Gyre_Data(Gyre);
	Gyre->begin_yaw=Gyre->Data.Yaw;
	Gyre->set_yaw=Gyre->begin_yaw;
	Gyre->mode=Gray_MODE_BLANCE;
	Gyre->trend=Gyre_STOP;
	Gyre->weight=0;
}

void Gyre_PID_Init(Gyre_t* Gyre,uint32_t maxout,
										uint32_t intergral_limit,
										float kp,
										float ki,
										float kd)
{
	PID_struct_init(&(Gyre->pid),POSITION_PID,maxout,intergral_limit,kp,ki,kd);
}

void Gyre_PID_Mode(Gyre_t* Gyre,_e_GyrePIDMode mode)
{
	Gyre->mode=mode;
}

void Gyre_Turn_Left(Gyre_t* Gyre,uint8_t yaw)
{
	if(Gyre->set_yaw+yaw>360)
	{
		Gyre->set_yaw+=yaw-360;
	}
	else
	{
		Gyre->set_yaw+=yaw;
	}
	HAL_Delay(5000);
}

void Gyre_Turn_Right(Gyre_t* Gyre,uint8_t yaw)
{
	if(Gyre->set_yaw-yaw<0)
	{
		Gyre->set_yaw-=yaw+360;
	}
	else
	{
		Gyre->set_yaw-=yaw;
	}
	HAL_Delay(5000);
}





uint8_t Get_Gyre_Data(Gyre_t* Gyre)
{
	uint8_t RollL,RollH,PitchL,PitchH,YawL,YawH,VL,VH,SUM,SUM_D;
	uint16_t Roll1,Pitch1,Yaw1,V1;
	float Roll2,Pitch2,Yaw2,V2;
	uint8_t i;
	for(i=0;i<get_lon;i++)
	{
		if(i>get_lon-data_lon)break;
		if(pData[i]==85)
		{
			if(pData[i+1]==83)
			{
				RollL=pData[i+2];
				RollH=pData[i+3];
				PitchL=pData[i+4];
				PitchH=pData[i+5];
				YawL=pData[i+6];
				YawH=pData[i+7];
				VL=pData[i+8];
				VH=pData[i+9];
				SUM=pData[i+10];
				
				SUM_D=0x55+0x53+RollL+RollH+PitchL+PitchH+YawL+YawH+VL+VH;
				if(SUM==SUM_D)
				{
					Roll1=RollH;
					Roll1=Roll1<<8;
					Roll1+=RollL;
					
					
					Pitch1=PitchH;
					Pitch1=Pitch1<<8;
					Pitch1+=PitchL;
					
					Yaw1=YawH;
					Yaw1=Yaw1<<8;
					Yaw1+=YawL;
					
					V1=VH;
					V1=V1<<8;
					V1+=VL;
					
					Roll2=(double)Roll1;
					Gyre->Data.Roll=Roll2/32768*180;
					
					Pitch2=(double)Pitch1;
					Gyre->Data.Pitch=Pitch2/32768*180;
					
					Yaw2=(double)Yaw1;
					Gyre->Data.Yaw=Yaw2/32768*180;
					
					V2=(double)V1;
					Gyre->Data.V=V2/32768*180;
					return 1;
				}
			}	
		}
	}
	return 0;
}
